%% 读取IQ数据并预处理
clear;clc;
data = load('data.txt');
I_raw = data(:, 1);
Q_raw = data(:, 2);

% 归一化处理（范围[-1,1]）
I = I_raw / max(abs(I_raw));
Q = Q_raw / max(abs(Q_raw));

% ========== 验证归一化效果 ==========
figure;
scatter(I_raw, Q_raw, 10, 'filled');
axis equal;
title('原始IQ信道星座图');
xlabel('I通道'); ylabel('Q通道');
grid on;

figure;
scatter(I, Q, 10, 'filled');
axis equal;
title('鲁棒归一化星座图');
xlabel('I通道'); ylabel('Q通道');
grid on;

avg_magnitude = mean(sqrt(I.^2 + Q.^2));
fprintf('平均幅值: %.4f\n', avg_magnitude);

% 计算差分（注意边界处理）
dI = diff(I);       % ΔI[k] = I[k] - I[k-1]
dQ = diff(Q);       % ΔQ[k] = Q[k] - Q[k-1]

% 相位计算（向量化实现）
n = length(I);
k_indices = 2:n;    % 有效采样点索引
numerator = I(k_indices) .* dQ - dI .* Q(k_indices);
denominator = I(k_indices).^2 + Q(k_indices).^2 + eps; % 避免除零

% 累积相位计算
Phi_theta = zeros(n, 1);
Phi_theta(k_indices) = cumsum(numerator ./ denominator);

% 时间向量生成（示例参数）
fs = 200;           % 采样率200Hz
t = (0:n-1)/fs;     % 时间向量

% 心脏位移计算
c = 3e8; % 光速
f_c = 24e9; % 载波频率
lambda = c/f_c; % 载波波长
x = lambda*Phi_theta/(4*pi);
x_norm = x/max(abs(x)); % 归一化（范围0，1）

% 添加n阶中值滤波（窗口长度=n+1）
x_norm_medfilt = medfilt1(x_norm, 8, 'truncate'); % 对称延拓边界

%% 多项式拟合呼吸信号
M = 6; % 多项式阶数（根据论文建议）
k = 1:n; % 采样点索引（自变量）
p = polyfit(k', x_norm_medfilt, M); % 获取拟合系数
breath_signal = polyval(p, k); % 生成拟合呼吸信号

% 计算均方误差（公式10）
mse = mean((breath_signal' - x_norm_medfilt).^2);
fprintf('多项式拟合MSE: %.4f\n', mse);

% 提取心跳信号（原始位移 - 呼吸信号）
heart_signal = x_norm_medfilt - breath_signal';
heart_signal = (heart_signal - min(heart_signal)) / (max(heart_signal) - min(heart_signal));%缩放至[0,1]

%% 可视化（新增呼吸信号对比）
figure
subplot(4,1,1)
plot(t, x_norm_medfilt, 'b', t, breath_signal, 'r', 'LineWidth', 1.5)
title('位移信号与呼吸拟合')
legend('中值滤波位移','多项式拟合'), xlabel('时间(s)'), grid on

subplot(4,1,2)
plot(t, x_norm_medfilt, 'g', 'LineWidth', 1.5)
title('呼吸消除前的原始心跳')
xlabel('时间(s)'), ylabel('幅值'), grid on

%% 修改后的周期检测与关键点标注
% 检测B点（峰值）和周期最低点
[peaks, locs] = findpeaks(heart_signal, 'MinPeakHeight', 0.6*max(heart_signal), 'MinPeakDistance', 0.25*fs);
% 检测每个周期的最低点（谷值）
lowest_points = zeros(length(locs), 2); % [时间, 幅值]
for i = 1:length(locs)
    if i == 1
        search_range = 1:locs(i); % 第一个周期从信号起点到第一个B点
    else
        search_range = locs(i-1):locs(i); % 其他周期从前一B点到当前B点
    end
    [min_val, min_idx] = min(heart_signal(search_range));
    lowest_points(i,:) = [t(search_range(1)+min_idx-1), min_val];
end

% 预分配存储
A_points = zeros(length(locs), 2);
B_points = [t(locs)', peaks];

for i = 1:length(locs)
    % 计算周期持续时间（从最低点到B点）
    cycle_duration = t(locs(i)) - lowest_points(i,1);
  
    % A点位置（最低点到B点的2/3时间处）
    A_time = lowest_points(i,1) + 2/3 * cycle_duration;
  
    % 查找最近采样点
    [~, A_idx] = min(abs(t - A_time));
    A_points(i,:) = [t(A_idx), heart_signal(A_idx)];
end

%% 可视化增强
subplot(4,1,3)
plot(t, heart_signal, 'm', 'LineWidth', 1.5)
hold on

% 标注最低点
scatter(lowest_points(:,1), lowest_points(:,2), 100, 's', 'filled',...
       'MarkerEdgeColor','k', 'MarkerFaceColor','b', 'DisplayName','周期起点（最低点）')

% 标注关键点
scatter(B_points(:,1), B_points(:,2), 100, 'v', 'filled',...
       'MarkerEdgeColor','k', 'MarkerFaceColor','y', 'DisplayName','B点(峰值)')
scatter(A_points(:,1), A_points(:,2), 100, 'o', 'filled',...
       'MarkerEdgeColor','k', 'MarkerFaceColor','g', 'DisplayName','A点(2/3位置)')

% 绘制周期分割线（从最低点到B点）
for i = 1:length(locs)
    line([lowest_points(i,1) lowest_points(i,1)], ylim,...
        'Color','b', 'LineStyle','--', 'HandleVisibility','off')
end
title('改进后的关键点标注（基于最低点-峰值）')
legend('show')

%% 控制台输出更新
fprintf('\n心跳周期参数表：')
fprintf('\n周期\t最低点时间\tB点时间\tA点时间\tA点振幅\t周期时长(s)\n')
for i = 1:length(locs)
    fprintf('%2d\t%.3f\t\t%.3f\t%.3f\t%.3f\t%.3f\n',...
            i, lowest_points(i,1), B_points(i,1), A_points(i,1), A_points(i,2),cycle_duration)
end

subplot(4,1,4)
[p_heart, f] = pwelch(heart_signal, [], [], [], fs);
plot(f, 10*log10(p_heart))
xlim([0 5]), grid on
title('心跳信号功率谱'), xlabel('频率(Hz)')